#include "camera_driver/hikrobot.hpp"

#include <libusb-1.0/libusb.h>
#include <stdexcept>
#include <chrono>
#include <thread>
#include <unordered_map>

using namespace std::chrono_literals;

namespace io
{
  HikRobot::HikRobot(double exposure_ms, double gain, const std::string &vid_pid, const rclcpp::Logger &logger)
      : CameraBase(logger),
        exposure_us_(exposure_ms * 1e3), // ms转us
        gain_(gain),
        handle_(nullptr), // 初始化为空句柄
        vid_(-1),
        pid_(-1),
        width_(0),
        height_(0)
  {
    set_vid_pid(vid_pid);
    // 初始化libusb
    if (libusb_init(NULL))
    {
      RCLCPP_WARN(logger_, "Unable to init libusb!");
    }
  }

  // 析构函数：确保资源释放
  HikRobot::~HikRobot()
  {
    close();           // 关闭相机
    libusb_exit(NULL); // 释放libusb资源
    RCLCPP_INFO(logger_, "HikRobot destroyed.");
  }

  // read()直接取图
  void HikRobot::read(sensor_msgs::msg::Image &img)
  {
    MV_FRAME_OUT raw_frame; // 创建帧结构体

    // 设置默认为空图像
    img.width = 0;    // 宽度为0
    img.height = 0;   // 高度为0
    img.data.clear(); // 清空数据缓冲区
    img.step = 0;     // 每行字节数为0
    img.encoding = "";

    // 1. 检查相机句柄是否有效
    if (handle_ == nullptr)
    {
      throw std::runtime_error("Camera handle invalid (not opened)");
    }

    // 2. 从相机获取一帧图像（超时1000ms）
    unsigned int ret = MV_CC_GetImageBuffer(handle_, &raw_frame, 1000);
    if (ret != MV_OK)
    {
      throw std::runtime_error("Failed to get image buffer (code: " + std::to_string(ret) + ")");
    }

    // 4. 计算当前帧所需内存大小（RGB8格式：宽×高×3字节/像素）
    size_t frame_width = raw_frame.stFrameInfo.nWidth;
    size_t frame_height = raw_frame.stFrameInfo.nHeight;
    size_t data_size = frame_width * frame_height * 3;

    // 5. 为目标消息img.data预留/调整内存（避免频繁分配）
    if (img.data.capacity() < data_size)
    {
      img.data.reserve(data_size); // 仅在容量不足时预留
    }
    img.data.resize(data_size); // 确保数据区大小与当前帧匹配

    // 6. 配置像素格式转换参数
    convert_param_.pDstBuffer = img.data.data();
    convert_param_.nDstBufferSize = data_size;
    convert_param_.pSrcData = raw_frame.pBufAddr;
    convert_param_.nSrcDataLen = raw_frame.stFrameInfo.nFrameLen;
    convert_param_.enSrcPixelType = raw_frame.stFrameInfo.enPixelType;

    // 7. 执行格式转换（直接将RGB8数据写入img.data）
    MV_CC_ConvertPixelType(handle_, &convert_param_);
    // 8. 填充目标消息的关键字段
    img.width = frame_width;
    img.height = frame_height;
    img.step = frame_width * 3;
    img.encoding = "rgb8";

    // 9. 释放相机SDK的原始图像缓冲区
    ret = MV_CC_FreeImageBuffer(handle_, &raw_frame);
    if (ret != MV_OK)
    {
      RCLCPP_WARN(logger_, "Failed to free image buffer (code: %u)", ret);
    }
  }

  // 实现基类open()：5次重试逻辑
  bool HikRobot::open()
  {
    const int MAX_RETRIES = 5;
    for (int i = 0; i < MAX_RETRIES; ++i)
    {
      if (try_open())
      {
        RCLCPP_INFO(logger_, "HikRobot opened successfully (attempt %d/%d)", i + 1, MAX_RETRIES);
        return true;
      }

      // 最后一次失败无需等待
      if (i < MAX_RETRIES - 1)
      {
        RCLCPP_WARN(logger_, "Retrying to open HikRobot... (%d/%d)", i + 2, MAX_RETRIES);
        std::this_thread::sleep_for(1s); // 间隔1秒重试
      }
    }
    RCLCPP_ERROR(logger_, "Failed to open HikRobot after %d attempts", MAX_RETRIES);
    return false;
  }

  // 单次打开相机尝试
  bool HikRobot::try_open()
  {
    try
    {
      // 1. 关闭已有连接
      if (handle_ != nullptr)
      {
        close();
      }

      // 2. 枚举设备
      MV_CC_DEVICE_INFO_LIST device_list;
      unsigned int ret = MV_CC_EnumDevices(MV_USB_DEVICE, &device_list);
      if (ret != MV_OK)
      {
        throw std::runtime_error("Device enumeration failed (code: " + std::to_string(ret) + ")");
      }
      if (device_list.nDeviceNum == 0)
      {
        throw std::runtime_error("No HikRobot cameras found");
      }

      // 3. 创建句柄并打开设备
      ret = MV_CC_CreateHandle(&handle_, device_list.pDeviceInfo[0]);
      if (ret != MV_OK)
      {
        throw std::runtime_error("Create handle failed (code: " + std::to_string(ret) + ")");
      }
      ret = MV_CC_OpenDevice(handle_);
      if (ret != MV_OK)
      {
        MV_CC_DestroyHandle(handle_); // 清理句柄
        handle_ = nullptr;
        throw std::runtime_error("Open device failed (code: " + std::to_string(ret) + ")");
      }
      // Get camera infomation
      MV_CC_GetImageInfo(handle_, &img_info_);

      // Init convert param
      convert_param_.nWidth = img_info_.nWidthValue;
      convert_param_.nHeight = img_info_.nHeightValue;
      convert_param_.enDstPixelType = PixelType_Gvsp_RGB8_Packed;

      // 4. 配置相机参数
      if (!set_enum_value("BalanceWhiteAuto", MV_BALANCEWHITE_AUTO_CONTINUOUS))
      {
        throw std::runtime_error("Failed to set white balance auto");
      }
      if (!set_enum_value("ExposureAuto", MV_EXPOSURE_AUTO_MODE_OFF))
      {
        throw std::runtime_error("Failed to disable auto exposure");
      }
      if (!set_enum_value("GainAuto", MV_GAIN_MODE_OFF))
      {
        throw std::runtime_error("Failed to disable auto gain");
      }
      if (!set_exposure(exposure_us_ / 1e3))
      { // 转换为ms
        throw std::runtime_error("Failed to set exposure");
      }
      if (!set_gain(gain_))
      {
        throw std::runtime_error("Failed to set gain");
      }
      ret = MV_CC_SetFrameRate(handle_, 150); // 设置帧率
      if (ret != MV_OK)
      {
        throw std::runtime_error("Set frame rate failed (code: " + std::to_string(ret) + ")");
      }

      // 5. 开始取流
      ret = MV_CC_StartGrabbing(handle_);
      if (ret != MV_OK)
      {
        close(); // 清理资源
        throw std::runtime_error("Start grabbing failed (code: " + std::to_string(ret) + ")");
      }

      return true;
    }
    catch (const std::exception &e)
    {
      RCLCPP_WARN(logger_, "Open attempt failed: %s", e.what());
      reset_usb(); // 重置USB尝试恢复
      return false;
    }
  }

  // 实现基类close()：释放相机资源
  void HikRobot::close()
  {
    if (handle_ == nullptr)
      return;

    // 停止取流
    MV_CC_StopGrabbing(handle_);
    // 关闭设备
    MV_CC_CloseDevice(handle_);
    // 销毁句柄
    MV_CC_DestroyHandle(handle_);

    handle_ = nullptr; // 置空句柄
    width_ = 0;
    height_ = 0;
    RCLCPP_INFO(logger_, "HikRobot closed");
  }

  // 解析VID/PID（与MindVision逻辑一致）
  void HikRobot::set_vid_pid(const std::string &vid_pid)
  {
    auto index = vid_pid.find(':');
    if (index == std::string::npos)
    {
      RCLCPP_WARN(logger_, "Invalid vid_pid format: \"%s\" (expected \"vid:pid\")", vid_pid.c_str());
      return;
    }

    auto vid_str = vid_pid.substr(0, index);
    auto pid_str = vid_pid.substr(index + 1);

    try
    {
      vid_ = std::stoi(vid_str, 0, 16); // 十六进制转整数
      pid_ = std::stoi(pid_str, 0, 16);
    }
    catch (const std::exception &)
    {
      RCLCPP_WARN(logger_, "Invalid vid_pid values: \"%s\"", vid_pid.c_str());
      vid_ = -1;
      pid_ = -1;
    }
  }

  // USB重置
  void HikRobot::reset_usb() const
  {
    if (vid_ == -1 || pid_ == -1)
    {
      RCLCPP_WARN(logger_, "Cannot reset USB - invalid VID/PID");
      return;
    }

    libusb_device_handle *usb_handle = libusb_open_device_with_vid_pid(NULL, vid_, pid_);
    if (!usb_handle)
    {
      RCLCPP_WARN(logger_, "Failed to open USB device (VID: 0x%04X, PID: 0x%04X)", vid_, pid_);
      return;
    }

    if (libusb_reset_device(usb_handle) != 0)
    {
      RCLCPP_WARN(logger_, "USB reset failed (VID: 0x%04X, PID: 0x%04X)", vid_, pid_);
    }
    else
    {
      RCLCPP_INFO(logger_, "USB reset successful (VID: 0x%04X, PID: 0x%04X)", vid_, pid_);
    }

    libusb_close(usb_handle);
  }

  // 设置浮点参数
  bool HikRobot::set_float_value(const std::string &name, double value)
  {
    if (handle_ == nullptr)
    {
      RCLCPP_WARN(logger_, "Cannot set %s - camera not open", name.c_str());
      return false;
    }

    unsigned int ret = MV_CC_SetFloatValue(handle_, name.c_str(), value);
    if (ret != MV_OK)
    {
      RCLCPP_WARN(logger_, "Set %s failed (value: %f, code: %#x)", name.c_str(), value, ret);
      return false;
    }
    return true;
  }

  // 设置枚举参数
  bool HikRobot::set_enum_value(const std::string &name, unsigned int value)
  {
    if (handle_ == nullptr)
    {
      RCLCPP_WARN(logger_, "Cannot set %s - camera not open", name.c_str());
      return false;
    }

    unsigned int ret = MV_CC_SetEnumValue(handle_, name.c_str(), value);
    if (ret != MV_OK)
    {
      RCLCPP_WARN(logger_, "Set %s failed (value: %u, code: %#x)", name.c_str(), value, ret);
      return false;
    }
    return true;
  }

  // 单独处理曝光设置（ms转us）
  bool HikRobot::set_exposure(double exposure_ms)
  {
    if (exposure_ms < 0.1 || exposure_ms > 10000.0)
    { // 海康曝光范围更广
      RCLCPP_WARN(logger_, "Exposure out of range (0.1~10000 ms)");
      return false;
    }
    double exposure_us = exposure_ms * 1e3;
    bool success = set_float_value("ExposureTime", exposure_us);
    if (success)
    {
      exposure_us_ = exposure_us; // 更新缓存
    }
    return success;
  }

  // 单独处理增益设置
  bool HikRobot::set_gain(double gain)
  {
    if (gain < 0.0 || gain > 48.0)
    {
      RCLCPP_WARN(logger_, "Gain out of range (0.0~48.0)");
      return false;
    }
    bool success = set_float_value("Gain", gain);
    if (success)
    {
      gain_ = gain; // 更新缓存
    }
    return success;
  }

  // 实现基类参数设置接口
  bool HikRobot::set_param(const std::string &param_name, double value)
  {
    if (param_name == "exposure_ms")
    {
      return set_exposure(value);
    }
    else if (param_name == "gain")
    {
      return set_gain(value);
    }
    else
    {
      RCLCPP_WARN(logger_, "HikRobot does not support param: %s", param_name.c_str());
      return false;
    }
  }
} // namespace io
